/* DESCRIPTION: The first pdsp is programmed to pass packets to the
//              QM using flow 0. Only packets which match one of two
//              destination mac addresses are forwarded.
//
//              The two mac addresses are run time configurable and
//              are passed in through a packet from the host. This packet
//              must have swinfo0 set to the magic value 0x0CEC11E0
//
//              The packet data contains the mac addresses to match. The
//              data arrives as a byte stream and it must be formated
//              as follows:
//
//       /---------------------------------------------------------------\
//       |   mac 0 msb    |  mac 0 byte 1 | mac 0 byte 2 | mac 0 byte 3  |
//       |   mac 0 byte 4 |  mac 0 lsb    |    rsvd      |  rsvd         |
//       |   mac 1 msb    |  mac 1 byte 1 | mac 1 byte 2 | mac 1 byte 3  |
//       |   mac 1 byte 4 |  mac 1 lsb    |    rsvd      |  rsvd         |
//       \---------------------------------------------------------------/
//
//
//         Feedback: The following addresses are used in local memory which
//         can be read by the host to verify the operation of the firmware.
//
//         0x4:2000    64 bytes of the last packet read that was forwarded to the qm
//         0x4:4000    64 bytes of the last packet read that was discarded
//
//         The mailbox register contain the following values:
//         mailbox 0:  Main loop counter
//         mailbox 1:  Number of packets received
//         mailbox 2:  Number of packets forwarded to host
//         mailbox 3:  Number of packets discarded.
//
//   The registers are assigned as follows:
//
//     r0      | scratch
//     r1      | scratch
//     r2      | scratch
//     r3
//     r4  |CDE control
//     r5  |
//     r6      |
//     r7      |
//     r8      |  CDE Data window 0
//     r9      |   Always holds the first 32
//     r10     |   bytes of the packet
//     r11     |
//     r12     |
//     r13     |
//     r14         |
//     r15         |
//     r16         |  CDE Data window 1
//     r17         |   Always holds the second 32
//     r18         |   bytes of the packet
//     r19         |
//     r20         |
//     r21         |
//     r22  | Main loop counter
//     r23  | Count of packets received
//     r24  | Count of packets matching mac address 0 and forwarded to host
//     r25  | Count of packets matching mac address 1 and forwarded to host
//     r26  | Count of packets that were discarded
//     r27  | Current mac 0, 4 most significant bytes
//     r28  | Current mac 0, 2 least significant bytes
//     r29  | Current mac 1, 4 most significant bytes
//     r30  | Current mac 1, 2 least significant bytes, r30.w0 has the destination queue for matched packets
//     r31  | flags
*/
#include "L1.h"
#include <string.h>


/*  .origin         0
    .entrypoint     bootStart

    .assign struct_CdeCmdPkt,   r4,  r5,  s_cdeCmdPkt
    .assign struct_CdeCmdWd,    r4,  r5,  s_cdeCmdWd
    .assign struct_bootStats,   r22, r30, s_stats
    .assign struct_pktDsc,      r6,  r13, s_pktDsc
*/
    BootStats   stats_c0   __attribute__((cregister("STATUS", near), peripheral));//c0
    PktDsc      pktDsc_c2  __attribute__((cregister("DESC_II", near), peripheral));//c2
    PktDsc      pktDsc_c4  __attribute__((cregister("DESC_IO", near), peripheral));//c4
    CdeCmdWd    cdeCmdWd;
    CdeCmdPkt   cdeCmdPkt;

    volatile register unsigned int __R31;

int main(void) {
/*bootStart:
    // Initialize the stats
    zero &s_stats, SIZE(s_stats)
    sbco s_stats,  c0,              0,      SIZE(s_stats)
 */
    memset(&stats_c0, 0, sizeof(BootStats));
top:
    stats_c0.mainLoopCounter++;

l1:
/*   qbbc  top, r31.tStatus_CDENewPacket


    // Read in the descriptor. Jump to config if swinfo0 does not match the magic value
    xin XID_CDEDATA, s_pktDsc,  SIZE(s_pktDsc)
    mov  r2.w2,   PA_MAGIC_ID >> 16
    mov  r2.w0,   PA_MAGIC_ID & 0xffff
    qbne l2,      s_pktDsc.swinfo0,        r2

    // If the data size is too small discard the packet
    qble config, s_pktDsc.pktDataSize, 16
        mov  s_cdeCmdPkt.operation,    CDE_CMD_PACKET_FLUSH
        xout XID_CDECTRL,              s_cdeCmdPkt,          SIZE(s_cdeCmdWd)
        jmp top
*/
    if (0 == (__R31 & CDENewPacket))
        goto top;

    if (PA_MAGIC_ID != pktDsc_c2.swinfo0)// == / !=
        goto l2;

    if (pktDsc_c2.pktDataSize < 16)
        goto config;

    cdeCmdPkt.operation = CDE_CMD_PACKET_FLUSH;
    ;
    goto top;

l2:
/*
    add s_stats.packetsReceivedCount, s_stats.packetsReceivedCount, 1
    sbco  s_stats.packetsReceivedCount, c4, 4, 4

    // Scroll past the descriptor and any control data
    mov  s_cdeCmdWd.operation,  CDE_CMD_ADVANCE_TO_PACKET
    xout XID_CDECTRL,           s_cdeCmdWd,                 SIZE(s_cdeCmdWd)

    // Read in the 1st 64 bytes of data from the packet
    xin XID_CDEDATA,  r6,  32

    mov  s_cdeCmdWd.operation,   CDE_CMD_ADVANCE
    mov  s_cdeCmdWd.byteCount,   32
    xout XID_CDECTRL,            s_cdeCmdWd,          SIZE(s_cdeCmdWd)

    xin XID_CDEDATA, r14,  32

    // Compare the destination mac to the two allowed addresses
    // Check mac address 0
    qbne  checkMac1,  r27,  r6
        qbne checkMac1, r28.w2, r7.w2

            // The packet matches mac address 0
            add s_stats.packetsMatchMac0Count, s_stats.packetsMatchMac0Count, 1
            jmp packetToHost
*/
    stats_c0.packetsReceivedCount++;

    cdeCmdWd.operation = CDE_CMD_ADVANCE_TO_PACKET;
    ;



checkMac1:

/*    // Compare the destination mac to mac 1
    qbne  noMacMatch, r29,  r6
        qbne noMacMatch, r30.w2, r7.w2

            // The packet matches mac address 1
            add s_stats.packetsMatchMac1Count, s_stats.packetsMatchMac1Count, 1
            jmp packetToHost


noMacMatch:

    add  s_stats.packetsDiscarded, s_stats.packetsDiscarded, 1
    sbco s_stats.packetsDiscarded, c4,  12,  4
    mov  s_cdeCmdPkt.operation,    CDE_CMD_PACKET_FLUSH
    xout XID_CDECTRL,              s_cdeCmdPkt,          SIZE(s_cdeCmdWd)
    sbco r6,                       c2,                   0,                 64
    jmp  top



packetToHost:

    add  r1, s_stats.packetsMatchMac0Count, s_stats.packetsMatchMac1Count
    sbco r1, c4, 8, 4

    mov  s_cdeCmdPkt.operation,     CDE_CMD_PACKET_ADVANCE
    mov  s_cdeCmdPkt.threadId,      THREADID_CDMA
    mov  s_cdeCmdPkt.psInfoSize,    0
    mov  s_cdeCmdPkt.destQueue,     r30.w0
    mov  s_cdeCmdPkt.optionsFlag,   CDE_FLG_SET_THREADID | CDE_FLG_SET_FLOWID | CDE_FLG_SET_DESTQUEUE
    mov  s_cdeCmdPkt.flowId,        0
    xout XID_CDECTRL,               s_cdeCmdPkt,            SIZE(s_cdeCmdPkt)
    sbco r6,                        c1,                     0,                  64
    jmp  top
*/


config:
/*
    // Scroll past the descriptor and any control data
    mov  s_cdeCmdWd.operation,  CDE_CMD_ADVANCE_TO_PACKET
    xout XID_CDECTRL,           s_cdeCmdWd,                 SIZE(s_cdeCmdWd)

    // Read in the 16 bytes of data
    xin XID_CDEDATA, r14,  16
    mov r27, r14
    mov r28, r15
    mov r29, r16
    mov r30, r17

    // Discard the packet
    mov  s_cdeCmdPkt.operation,    CDE_CMD_PACKET_FLUSH
    xout XID_CDECTRL,              s_cdeCmdPkt,          SIZE(s_cdeCmdWd)

    jmp top
*/

	return 0;
}
